#include "RayTracer.h"
#include <maya/MItDag.h>
#include <maya/MDagPath.h>

#include <maya/MStatus.h>
#include <maya/MFnDagNode.h>
#include <maya/MFnCamera.h>
#include <maya/MFnLight.h>
#include <maya/MColor.h>
#include <maya/MFnNurbsSurface.h>
#include <maya/MIOStream.h>
#include <maya/MPoint.h>
#include <maya/MVector.h>
#include <maya/MString.h>
#include <maya/MTransformationMatrix.h>
#include <maya/MMatrix.h>

#include <maya/MItDependencyNodes.h>
#include <maya/MLibrary.h>
#include <maya/MFileIO.h>

#include <maya/MFnMesh.h>
#include <maya/MFnMeshData.h>
#include <maya/MPointArray.h>
#include <maya/MItMeshVertex.h>
//#include <maya/MFnPlugin.h>
#include <maya/MImage.h>
#include <maya/MFloatMatrix.h>
#include <maya/MCommonSystemUtils.h>
#include <vector>

inline int RoundToInt(double val)
{
	return (int)floor(val + 0.5);
}

void* RayTracer::creator() 
{ 
	return new RayTracer; 
}
 
MStatus RayTracer::doIt(const MArgList& argList) 
{
	MItDag::TraversalType	traversalType = MItDag::kDepthFirst;
	MFn::Type				filter              = MFn::kInvalid;
	//MStatus					status;
	bool					quiet = false;

  cout << endl << endl << endl << endl;
  cout << "==============================" << endl;

  // get all centers of mass
	std::vector<MPoint> com_vector;
  MItDependencyNodes mesh_it(MFn::kMesh);
  while(!mesh_it.isDone())
  {
    MObject obj = mesh_it.item();
    cout << obj.apiTypeStr() << endl;
    MDagPath aPath = MDagPath::getAPathTo(obj);
    MPoint com;
    getCenterOfMass(aPath, true, com);
    com_vector.push_back(com);
    cout << "Com " << com_vector.size() << ": " << com.x << " " << com.y << " " << com.z << endl;
    mesh_it.next();
  }
  
  // look at all cameras
  bool found_non_default_ortho_cam = false;
  MItDependencyNodes cam_it(MFn::kCamera);
	MMatrix	camera_matrix;
	MMatrix	inverse_camera_matrix;
  MFloatMatrix mat_proj;
	double windowAspect = 1.0;
	double left;
	double right;
	double bottom;
	double top;
  
  while(!cam_it.isDone())
  {
    MObject obj = cam_it.item();
    MDagPath aPath = MDagPath::getAPathTo(obj);
    MStatus status;
	  MFnCamera camera (aPath, &status);
		if ( !status ) {
			status.perror("MFnCamera constructor");
			continue;
		}
    std::cout << "Found camera " << camera.name().asChar() << std::endl;

    // skip if it's one of the default cameras
    if((MString("perspShape") == camera.name()) ||
      (MString("topShape") == camera.name()) ||
      (MString("frontShape") == camera.name()) ||
      (MString("sideShape") == camera.name()))
    {
      std::cout << "Camera is default camera; skipping..." << std::endl;
      cam_it.next();
      continue;
    }
    if(!camera.isOrtho())
    {
      std::cout << "Camera is not orthographic; skipping..." << std::endl;
      cam_it.next();
      continue;
    }

    found_non_default_ortho_cam = true;

    // get the cam matrix, its inverse, and the projection matrix
		MObject	transformNode = aPath.transform(&status);
		MFnDagNode	transform (transformNode, &status);
    mat_proj = camera.projectionMatrix();
		camera_matrix = (transform.transformationMatrix());
		inverse_camera_matrix = camera_matrix.inverse();
		camera.getRenderingFrustum(windowAspect, left, right, bottom, top);
    cam_it.next();
  }

  if(!found_non_default_ortho_cam)
  {
    std::cout << "Found no non-default orthographic cameras. Exiting..." << std::endl;
    return MStatus::kFailure;
  }


  std::cout << "Ortho Cam Properties: " << std::endl;
  std::cout << "  {Left, Right, Bot, Top} = {" << left << ", " << right << ", " << bottom << ", " << top << "}" << std::endl;

	float mat_proj_arr[4][4];
	for(int i=0; i<4; ++i) {
		for(int j=0; j<4; ++j) {
			mat_proj_arr[i][j] = mat_proj(i,j);
		}
	}

	float mat_ptr[4][4];
	for(int i=0; i<4; ++i) {
		for(int j=0; j<4; ++j) {
			mat_ptr[i][j] = 0;
		}
	}
	mat_ptr[0][0] = 2.0/(right-left);
	mat_ptr[3][0] = -1*((right+left)/(right-left));
	mat_ptr[1][1] = 2.0/(top-bottom);
	mat_ptr[3][1] = -1*((top+bottom)/(top-bottom));
	mat_ptr[2][2] = -2.0/(1000.0-0.0001);
	mat_ptr[3][2] = -((1000+0.0001)/(1000-0.0001));
	mat_ptr[3][3] = 1.0;

  // this one made manually
	MMatrix ortho_proj(mat_ptr);
  // this one made from the proj mat via .projectionMatrix();
  MMatrix ortho_proj_api(mat_proj_arr);


  // create the image
  MStatus stat_img;
	MImage  img;
	int width                   = 1920; 
  int height                  = 1080;
	stat_img                    = img.create(width,height);
	int depth                   = img.depth();
	unsigned char* pixel_array  = img.pixels();
  memset(pixel_array, 0, width*height*depth);

  // for each center of mass, fill in the appropriate pixel
	for(int i = 0 ; i < com_vector.size() ; i++)
	{
    MPoint src = com_vector.at(i);
		MPoint p1 = src*inverse_camera_matrix;
		//MPoint p2 = p1*ortho_proj;
		//int x = RoundToInt((p2.x*width/2.0) + (width/2.0));
		//int y = RoundToInt((p2.y*height/2.0) + (height/2.0));

    MPoint p2_api = p1*ortho_proj_api;
		int x = RoundToInt((p2_api.x*width/2.0) + (width/2.0));
		int y = RoundToInt((p2_api.y*height/2.0) + (height/2.0));

    std::cout << "Pt " << i << ": (" << p1.x << ", " << p1.y << ", " << p1.z << ") => (" << x << ", " << y << ")" << std::endl;

		if((x < 0) || (x >= width) || (y < 0) || (y >= height))
		{
      std::cout << "COM outside of the image: " << "(" << x << "," << y << ")" << std::endl;
      continue;
		}

    pixel_array[y*width*depth + x*depth + 0] = 255;
    pixel_array[y*width*depth + x*depth + 1] = 255;
		pixel_array[y*width*depth + x*depth + 2] = 255;
	  pixel_array[y*width*depth + x*depth + 3] = 255;

	}

  // create the output directory
  MString user_dir = MCommonSystemUtils::getEnv(MString("MAYA_APP_DIR"));
  MString img_dir = user_dir;
  MString last_char = img_dir.substring(img_dir.length()-1, img_dir.length()-1);
  if((MString("/") != last_char) && (MString("\\") != last_char)){
    img_dir += MString("/");
  }
  img_dir += MString("img/");
  MCommonSystemUtils::makeDirectory(img_dir);

  // finally, write the file
  MString output_filename = img_dir + MString("scene.tif");
	stat_img = img.writeToFile(output_filename,"tif");
  if(stat_img) {
    MGlobal::displayInfo(MString("Successfully wrote scene file ")+output_filename);
  } else {
    MGlobal::displayInfo(MString("Error writing scene file ")+output_filename);
  }

  // release the image
  img.release();
  return MStatus::kSuccess;
}

bool RayTracer::getCenterOfMass(const MDagPath& dagPath, bool quiet, MPoint& com)
{
	MStatus		status;
	MObject		transformNode = dagPath.transform(&status);
	// This node has no transform - i.e., it's the world node
	if (!status && status.statusCode () == MStatus::kInvalidParameter)
		return false;
	MFnDagNode	transform (transformNode, &status);
	if (!status) {
		status.perror("MFnDagNode constructor");
		return false;
	}
	/*MTransformationMatrix	matrix (transform.transformationMatrix());

	double									threeDoubles1[3],threeDoubles2[3],threeDoubles3[3];
	MTransformationMatrix::RotationOrder	rOrder;

	matrix.getRotation (threeDoubles1, rOrder, MSpace::kWorld);
	matrix.getScale (threeDoubles2, MSpace::kWorld);
	matrix.getTranslation(MSpace::kWorld);*/

	MItMeshVertex it (dagPath);
	int count_vertices = 0;
	it.reset();
	while(!it.isDone())
	{
		MObject item = it.currentItem();
		MPoint pos = it.position(MSpace::kWorld);
		com += pos;
		it.next();
		count_vertices++; 
	}

	if(count_vertices != 0)
		com = com * (1.0/double(count_vertices));
	return true;
}
